#include "MeasurE616AlgFrontSe2Se8Edge.h"

MeasurE616AlgFrontSE2SE8Edge::MeasurE616AlgFrontSE2SE8Edge(int i):MeasurE616AlgBase(i)
{

}

void MeasurE616AlgFrontSE2SE8Edge::operator()(cv::Mat &srcImg, int id,std::map<string, float> &measRes,
                                              int fixtureId)
{

    //新螺纹孔边缘提取，返回圆参数  SE8模板图像的front图     \paras616\front\SE.xml的第7行

    //int SmallCircle(cv::Mat srcimg, const vector<int> paras, vector<float>& res, int flag);


    //SE8模板图像的front图  \paras616\front\SE.xml的第8行

    //int NewGetRectEdge(cv::Mat srcimg, const vector<int> paras, vector<float>& dists, vector<vector<cv::Point2f>>& contourres);


    //SE2front图 （setup1）  \paras616\front\SE.xml的第9行；

    //int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);


    std::vector<float> resVec;
    vector<vector<cv::Point2f>> contourres;
    std::vector<cv::Point2f> respts;

    {
        std::vector<int> tparas;
        int i=35;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(6,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();

        MeasurE616EdgeDetect obj;
        obj.SmallCircle(imgt, tparas, resVec, 1);
        std::vector<cv::Point2f> contours{cv::Point2f(resVec[0], resVec[1])};
        ImageCoordinateTransformation(contours,rangVec);

#ifdef MEASUR_E616_DRAW_IMG
        static int seId = 0;
        std::string imgName = ("se" + to_string(seId));
        imgName += ".jpg";

        cv::Mat srcImgTemp = srcImg.clone();
        for(int i = 0; i < contours.size(); i++) {
            cv::circle(srcImgTemp, contours[i], 2, 160, -1);
        }
        imwrite(imgName, srcImgTemp);

        seId++;
#endif
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        cv::Point2f ptsOut;
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y));
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            //resptVec[1].push_back(ptsOut);
        }

        resVec[0]=ptsOut.x,resVec[1]=ptsOut.y;

        //cout<<resVec.size()<<endl;
    }

    {
        std::vector<int> tparas;
        vector<float> dists;
        int i=35,tg=0;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(7,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        vector<vector<cv::Point2f>> contourres0;
        MeasurE616EdgeDetect obj;
        obj.NewGetRectEdge(imgt, tparas,dists,contourres0);
        for(auto &ci:contourres0)
        {
            ImageCoordinateTransformation(ci,rangVec);
        }


#ifdef MEASUR_E616_DRAW_IMG
        static int seId = 0;
        std::string imgName = ("se" + to_string(seId));
        imgName += ".jpg";

        cv::Mat srcImgTemp = srcImg.clone();
        for(int i = 0; i < contourres0.size(); i++) {
            for(int j = 0; j < contourres0[i].size(); ++j) {
                cv::circle(srcImgTemp, contourres0[i][j], 2, 160, -1);
            }
        }
        imwrite(imgName, srcImgTemp);

        seId++;
#endif
        contourres.resize(contourres0.size());
        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &ci:contourres0)
        {

           for(auto &cj:ci)
           {
               cv::Point2f ptsOut;
              CoordinateTransformationRelateToDot(datumBackVec[id], cj, ptsOut);
              contourres[tg].push_back(ptsOut);
           }
           tg++;
        }

    }

    {
        std::vector<int> tparas;
        int i=29;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(8,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> respts0;
        MeasurE616EdgeDetect obj;
        obj.LDmeasure(imgt, tparas,respts0);
        ImageCoordinateTransformation(respts0,rangVec);

#ifdef MEASUR_E616_DRAW_IMG
        static int seId = 0;
        std::string imgName = ("se" + to_string(seId));
        imgName += ".jpg";

        cv::Mat srcImgTemp = srcImg.clone();
        for(int i = 0; i < respts0.size(); i++) {
            cv::circle(srcImgTemp, respts0[i], 2, 160, -1);
        }
        imwrite(imgName, srcImgTemp);

        seId++;
#endif

        std::unique_lock<std::mutex> lock(*MeasurE616AlgBase::mtxptrVec[id]);
        while (!MeasurE616AlgBase::flagVec[id])
        {
            (*MeasurE616AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:respts0)
        {
            cv::Point2f ptsOut;
            CoordinateTransformationRelateToDot(datumBackVec[id], elem, ptsOut);
            respts.push_back(ptsOut);
        }

    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    MeasurE616EdgeDetect obj;
    vector<float> linepara;
    obj.LineFitLeastSquaresf(contourres[0],linepara);
    float dist1=fabs(linepara[0]*resVec[0]-resVec[1]+linepara[1])/sqrt(1+linepara[0]*linepara[0]);
    measRes.insert(std::make_pair(string("FAI118-SE8-Y-distance_pixel"), dist1));
    float fai118sey = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI118-SE8-Y-distance")], dist1);
    measRes.insert(std::make_pair(string("FAI118-SE8-Y-distance_real"), fai118sey));

    float fai8=0;
    for(int i=0;i<respts.size();++i){
        fai8+=respts[i].x/respts.size();
    }
    measRes.insert(std::make_pair(string("FAI8-SE2-X-distance_pixel"), fai8));
    float fai8sex = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI8-SE2-X-distance")], fai8);
    measRes.insert(std::make_pair(string("FAI8-SE2-X-distance_real"), fai8sex));

}
